#ifndef COMMON_H_
#define COMMON_H_

#include <stdbool.h>
#include <stdio.h>
#include <string.h>
#include <stdarg.h>
#include "main.h"
#include "std_basal.h"
#include "FreeRTOS.h"
#include "task.h"
#include "cmsis_os.h"

/*-------------------------------------------------------*/
#define M_PI	3.141592654
#define kMaxBufferLength	50
#define kMaxBezierControlSize	2
/*!<numerical macro definition*/
#define FLT32_ZERO   			(1.0e-7f)   			 
#define FLT64_ZERO   			(1.0e-14)   			 
#define LARGE_POS_VALUE			(10000000) 

#define MUSK_BIT0 (1<<0)
#define MUSK_BIT1 (1<<1)
#define MUSK_BIT2 (1<<2)
#define MUSK_BIT3 (1<<3)
#define MUSK_BIT4 (1<<4)
#define MUSK_BIT5 (1<<5)
#define MUSK_BIT6 (1<<6)
#define MUSK_BIT7 (1<<7)
#define MUSK_BIT8 (1<<8)
#define MUSK_BIT9 (1<<9)
#define MUSK_BIT10 (1<<10)
#define MUSK_BIT11 (1<<11)
#define MUSK_BIT12 (1<<12)
#define MUSK_BIT13 (1<<13)
#define MUSK_BIT14 (1<<14)
#define MUSK_BIT15 (1<<15)
#define MUSK_BIT16 (1<<16)
#define MUSK_BIT17 (1<<17)
#define MUSK_BIT18 (1<<18)
#define MUSK_BIT19 (1<<19)
#define MUSK_BIT20 (1<<20)
#define MUSK_BIT21 (1<<21)
#define MUSK_BIT22 (1<<22)
#define MUSK_BIT23 (1<<23)
#define MUSK_BIT24 (1<<24)
#define MUSK_BIT25 (1<<25)
#define MUSK_BIT26 (1<<26)
#define MUSK_BIT27 (1<<27)
#define MUSK_BIT28 (1<<28)
#define MUSK_BIT29 (1<<29)
#define MUSK_BIT30 (1<<30)
#define MUSK_BIT31 (1<<31)


/* unit mm */
typedef struct  
{
	double x;
	double y;
}stPoint;

typedef struct
{
	stPoint begin;
	stPoint end;
	stPoint arcCentre;
	float  radius;                                
	char   deltaAngle;
	bool accomplish;
	stPoint bezier_ctlPoint[kMaxBezierControlSize];	
	bool is_unload_point;
}stStarightLine;

typedef struct
{
	double x;
	double y;
	double orientation;
}stPose;

typedef struct
{
	float x;
	float y;
	float orientation;
}stPosefloat;

typedef struct
{
	int x;
	int y;
	int orientation;
}stPoseInt;

typedef struct{
	uint32 timer_stamp_4ms;
	uint32 timer_stamp_1us;
	stPosefloat camera_pose;
	stPosefloat camera_pose_err;
}stCameraInfo;

typedef struct
{
	s16 gyro_x;
	s16 gyro_y;
	s16 gyro_z;
	//s16 tempreture;
	//s16 res;
	s16 z_temp;
	s16 x_y_temp;
}stGyro3dRawData;

typedef struct 
{
	s16 x;
	s16 y;
	s16 z;
}st3dRawData;

typedef struct
{
	float x;
	float y;
	float z;
}st3dFloatData;

typedef struct
{
	s16 acc_x;
	s16 acc_y;
	s16 acc_z;
}stAcc3dRawData;

typedef struct
{
	uint32 timer_stamp_4ms;
	uint32 timer_stamp_1us;
	stGyro3dRawData gyro_data;
	stAcc3dRawData acc_data;	
}stIMU6DRawData;

typedef struct
{
	volatile uint32 head;
    volatile uint32 tail;  		
	stIMU6DRawData 	imu6D_raw_data[10];
}stIMU6DCycleBuffer;

typedef struct
{
	uint32 timer_stamp_4ms;
	uint32 timer_stamp_1us;
	stPosefloat odom_pose;
	uint32 encoder[4];
}stOdomInfo;


typedef struct
{
	uint32 timer_stamp_4ms;
	uint32 timer_stamp_1us;
	stPosefloat imu_pose;
	float null_offset;
	float scale_factor;
	float temperature;
	float prev_angular_velocity;
	float angular_velocity;
	float angular_acceleration;
	float angle;
}stImuInfo;

typedef struct
{
	uint32 flag;
	stCameraInfo	camera_data;
	stIMU6DCycleBuffer 	imu_buffer;	
	stOdomInfo  odom_info;
	stImuInfo 	imu_info;
}stPoseSensorInfo;

typedef struct
{
	stStarightLine line[kMaxBufferLength];
	bool is_startpoint_valid[kMaxBufferLength];
	bool is_temperature_checked;
	bool is_gyro_checked;
	bool is_camera_uper_updated;
	bool is_camera_uper_started;
	bool has_path_contructed_if_missing;
	bool is_backward[kMaxBufferLength];
	bool is_shelf_found[kMaxBufferLength];  /*FindShelfUp.*/
	bool is_shelf_aligned[kMaxBufferLength]; /*AlignedShelf.*/
	int current_index;
	int element_num;
	double lifter_start[kMaxBufferLength];
	double lifter_end[kMaxBufferLength];
}stPathPoints;

typedef struct
{
	double linear_v;
	double angular_v;
}stBaseVelocity;
typedef struct
{
	double lift_up;
	double lift_rotate;
}stLifterVelocity;

/*!< @todo detail description*/
typedef enum
{
	ErrorNormal,		//0
	ErrorTimeOut,		// 1
	ErrorInputCheck,	// 2
	ErrorOutOfRange,	// 3
	ErrorInternalStatus,// 4
	ErrorComType,		// 5
	ErrorNavigation,	// 6
	ErrorMechanic,		// 7
	ErrorLostBarcode,	// 8
	ErrorOutOfPath,		// 9
	ErrorImpossibleSwitch,// 10
	ErrorImpossibleElse,//11
	ErrorRimTrigered, 	//12
	ErrorDecodeFail,	//13
	ErrorWriteFlash,	//14
	ErrorInstruction,  	//15
	ErrorLinkLost,     	//16
	ErrorPositionLost, 	//17
	ErrorChargeDecode,  //18
	ErrirChargeTimeOut,	//19
	ErrorAngleOutOfControl,	//20
	ErrorAsLiningAsShelfAdjust,//21
	ErrorShelfIdDisparity,		//22
	ErrorNoChargeCurrent,   	//23
	ErrorLiftingTimeOut,   		 //24
	ErrorDroppingTimeOut,   		//25
	ErrorPackageOverLength,		 	// 26
	ErrorBeltNoIndex,			 	 // 27
	ErrorBeltType,				 	 // 28
	ErrorBeltFetchingOvertime,		 // 29
	ErrorBeltOutingOvertime,		 // 30
	ErrorBeltAdjustOvertime,		 // 31
	ErrorUsingParamillegal,			 // 32
	ErrorTurningTimeOut,		 	//33
	ErrorNutAngleOutOfRange,		 //34
	ErrorWriteEEPROM,
	ErrorFindBacord
}enErrorType;

typedef enum 
{
	VER_ROBOT             = 0,
	VER_ROBOTBOOT         = 1,
    VER_MOTOR1            = 2,
	VER_MOTOR2 		      = 3,
	VER_MOTOR3 		      = 4,
	VER_MOTOR4 		      = 5,
    VER_MOTOR1_BOOT       = 6,
    VER_MOTOR2_BOOT       = 7,
    VER_MOTOR3_BOOT       = 8,
    VER_MOTOR4_BOOT       = 9,
	VER_POWERBOARD_BOOT   = 10,
	VER_POWERBOARD 		  = 11,
	VER_FRONTBOARD        = 12,
	VER_BACKBOARD 		  = 13,
	VER_CPLD			  = 14,
	VER_PARAM_CONFIG      = 15,
	VER_PC = 16,
	VER_MAX
}enVersionIndex;


typedef enum 
{		
	GroundRect = 0,
	CeilRect = 1,
	GroundDecode = 2,
	CeilDecode = 3,
	GroundIdle = 4,
	CeilIdle   = 5,
	FrameSave  = 6,
	StopImageSave = 7,
	ErrorMode = 8,
}enCameraWorkMode;

typedef enum 
{	
	Default = 0,
	Idle,
	Rect,
	Decode,
	ImageUpload,	
}enCameraMode;

typedef enum /*MapOfCameraWorkMode.*/
{
	DspCommandIdle         = 0,
	DspCommandCeilDecode   = 1,
	DspCommandCeilRect     = 2,
	DspCommandGroundDecode = 3,
	DspCommandGroundRect   = 4,
	DspCommandFrameSave    = 5,
	DspCommandStopImageSave = 0x10,
	DspCommandGoundIdle    = 0xFD,
	DspCommandCeilIdle     = 0xFE,
	DspCmd548CeilDecode	 	 = 0x11,
	DspCmd548CeilRect		 	 = 0x10,
	DspCmd548GroundDecode	 = 0x22,
	DspCmd548GroundRect	 	 = 0x21,
}enDspCommandMode;

//typedef enum
//{
//	CeilShelfRect,
//	CeilChargeRect,
//	CeilShelfDecode
//}enCeilLocalization;

//typedef enum
//{
//	ShelfCeil,
//	ChargerCeil
//}enCeilType;
typedef enum 
{
	PositiveX,
	NegativeX,
	PositiveY,
	NegativeY,
	SamePoint,
	DirectionError
}enDirection;

typedef struct 
{
	unsigned short shelf_id;
	float orientation;
	unsigned short pixel_x;
	unsigned short pixel_y;
	unsigned short decode_x;
	unsigned short decode_y;
}stShelfInfo;

//typedef struct
//{
//	uint32_t decode_x;
//	uint32_t decode_y;
//	float orientation;
//	int pixel_x;
//	int pixel_y;	
//}stGroundInfo;

typedef struct
{
	/*parameters for robot control*/
	float  kBarcodeAngleAdjust ;
	float  kDownCameraMountX ;
	float  kDownCameraMountY ;
	float  kUpCameraAngleAdjust ;
	float  kUpCameraMountX ;
	float  kUpCameraMountY ;
	/*parameter for DSP image cut*/
	float kDSPEppi2hdelay ;
	float kDSPEppi2vdelay ;
	float kDSPEppi1hdelay ;
	float kDSPEppi1vdelay;
}stCameraCaliParam;
typedef struct
{
	/*Flash data flag, valid:0xEAEA5555*/
	bool Param_Valid;  
	/*parameters for Calibrate control*/
	unsigned int uCalibrateControl;//enable or disable boot function
	unsigned int kRobotID ;
	/*parameters for robot control*/
	float  kBarcodeAngleAdjust ;
	float  kDownCameraMountX ;
	float  kDownCameraMountY ;
	float  kUpCameraAngleAdjust ;
	float  kUpCameraMountX ;
	float  kUpCameraMountY ;
	/*parameter for DSP image cut*/
	float kDSPEppi2hdelay ;
	float kDSPEppi2vdelay ;
	float kDSPEppi1hdelay ;
	float kDSPEppi1vdelay;
}stCalibrationParam;


typedef struct
{
	unsigned int cmd_echo_lost;
	unsigned int encoder_echo_lost;
	unsigned int cmd_sent_count;
	unsigned int read_encoder_count;
	unsigned int read_current_count;
}stMotorLink;

enum enPathDeltaAnlge{S_LINE, ARC_CLOCKWISE, ARC_ANTI_CLOCKWISE,ARC_BEZIER};
enum enClockwise{ANTI_CLOCKWISE, CLOCKWISE};
enum enCondition{FALSE, TRUE, TIMEOUT};
typedef enum enTaskStatus
{
	TASK_PROCESSING =0x5A,
	TASK_FINISHED=0xCC,
	TASK_FAIL=0xEE 
}enTaskStatus;  

typedef enum
{
	PATH_GOING    = 0x5A,
	PATH_OUT      = 0xBB,   
	PATH_FINISHED = 0xCC
}enPathExcuteState;

typedef enum ROBOT_TASK_MODE
{
	GO_DELIVERING 					= 	0x11,   // deliver the shelf
	GO_RETURN     					= 	0x12,   // return the shelf
	QUEUING       					= 	0x14,   // queue
	GO_FETCHING   					= 	0x17,   // fetch the shelf
	GO_CHARGING   					= 	0x18,   // go to charge itself
	GYRO_CALIBRATE 					= 	0x1B,   // gyro calibration mode
	GO_REST       					= 	0x1C,   // go to backup zone   
	FIRMWARE_UPDATE  				= 	0x1D,   // firmware update   	
	GO_PICKINGUP						=		0x21,
	GO_UNLOADING						=		0x22,
	REMOTE_CONTROLLER				=		0x23,
	UPLOAD_IMAGE						= 	0x24,		
	//0x25 has been occupied by slam to inital map
	GO_FETCHING_STORE_BIN 	= 	0x26,		
	GO_DELIVERING_STORE_BIN = 	0x27,
	GO_RETURNING_STORE_BIN 	= 	0x28,
	IDLE          					= 	0x1F    // idle status
}ROBOT_TASK_MODE;

/* robot sub-task mode definition */
typedef enum ROBOT_TASK_SUBMODE
{
    CHARGE_GOPATH,          // walk to the charge station  
    CHARGE_DOCKING,         // last stage of the auto charge contact 
    CHARGE_DOCKED,          // contact success
    CHARGE_DOCKFAIL,        // contact fail
    CHARGE_DEPART,          // contact depart
    
    QUEUING_NEXT,
    QUEUING_INFIELD,
    QUEUING_OUTFIELD,
    QUEUING_BACKTOFIELD,
    QUEUING_PREBACKTOFIELD,
    QUEUING_TRUN,
    
    FETCH_SHELFNOTFUND,
    FETCH_SHELFIDERROR,
    FETCH_SHELFANGLEERROR,
    FETCH_GOPATH,
    FETCH_LIFTING,
    FETCH_SUCCESS,
    
    IDLE_IDLE,             
}ROBOT_TASK_SUBMODE;

/* robot motion mode definition */
typedef enum ROBOT_MOVING_MODE
{
	UNEXPEC_MOVE  = 0x00,   // unexpect value
	LINEAR_GOING  = 0x21,   // linear walk
    ARC_GOING     = 0x22,   // arc walk
	ANGULAR_TURN  = 0x24,   // turn at the origin 
	LIFTING       = 0x27,   // lift up the shelf
    DROPPING      = 0x28,   // drop down the shelf
	SHELF_ADJUST  = 0x29,   // revise the shelf angle
	STOP          = 0x2C,   // stop
    SHELF_TURN    = 0x2E,    // rotate the shelf angle
    ROBOT_SHELF_TURN = 0x2F,  //ROBOT ROTATE WITH SHELF
}ROBOT_MOVING_MODE;

/* robot sub motion mode definition */
typedef enum ROBOT_SUBMOVING_MODE
{
	UNEXPEC_SUBMOVE = 0x00,       // unexpect value
	ANGULAR_ACCE    = 0x37,       // arc speed up 
	ANGULAR_UNIFORM = 0x38,       // arc constant speed
	ANGULAR_DECE    = 0x3A,       // arc slow down 
	
	LINEAR_ACCE     = 0x31,       // linear speed up 
	LINEAR_UNIFORM  = 0x32,       // linear constant speed
	LINEAR_DECE     = 0x34,       // linear slow down 
	
	TURN_ACCE       = 0x3B,       // angular speed up 
	TURN_UNIFORM    = 0x3C,       // angular constant speed
	TURN_DECE       = 0x3F,       // angular slow down 
}ROBOT_SUBMOVING_MODE;

/* instruction from server */
typedef enum INSTRUCTION
{
    GO_FETCH             = 0x1111,   // fetch the shelf
	GO_DELIVER           = 0x2222,   // deliver the shelf
	GO_GIVEBACK          = 0X3333,   // return the shelf to its original position
	GO_CHARGE            = 0x4444,   // charge
	GO_FIXPOINT          = 0X5555,   // go to fix point 
	QUEUE_NEXT           = 0x6666,   // next queue point
	QUEUE_INFIELD        = 0x7777,   // go into Tianzige,and adjust the shelf orientation
	QUEUE_OUTFIELD       = 0x8888,   // get out of Tianzige
	
	QUEUE_BACKTOFIELD    = 0x9999,   // go into Tianzige,and adjust the shelf orientation again
	QUEUE_PREBACKTOFIELD = 0xAAAA,   // go into Tianzige again
	QUEUE_TURN           = 0xBBBB,   // adjust the orientation in Tianzige
	ROTATE_WITH_SHELF    = 0xBBCC,   // rotate robot with shelf
	ROTATE_WITH_SHELF_KEEPSYNC = 0xBBDD,  //rotate robot with shelf but keepsync
	LIFT_SHELF_TEST					=0xBBEE,

    NULL_INSTRUCT        = 0x0000,   // null instruction
    WAIT                 = 0xCCCC,   // waiting 
    MOTOR_STOP           = 0xDDDD,   // stop
	CANCEL_TASK          = 0xEEEE,

	GO_PICKUP						 = 0x1122,
	GO_UNLOAD				 		 = 0xFF01,
	
	REMOTE_UPDATE		 					= 0x4806,
	AUTO_CABLICATE		 					= 0x4809,	//TODO change value by protocol
	REMOTE_CONTROL							= 0x5000,
	GOTO_SLEEP           				= 0x5252, //goto sleep mode
	
	GO_FETCH_STORE_BIN					= 0x2233, 
	GO_DELIVER_STORE_BIN				= 0x2244,
	GO_RETURN_STORE_BIN					= 0x2255,
	PC_MOVE								= 0x0001,
	PC_MOVE_LOAD						= 0x0002,
	PC_SET_CAMERA						= 0x0003,
	PC_LIFT_UP							= 0x0004,
	PC_LIFT_DOWN						= 0x0005,
}INSTRUCTION;


typedef enum enPackageNum
{
	PACKAGE_NULL = 0x0000,
	PACKAGE_FRONT_FIRST = 0x0001,
	PACKAGE_REAR_FIRST = 0x0002, 
	PACKAGE_REAR_LAST = 0x0201, 
	PACKAGE_FRONT_LAST = 0x0102,
	PACKAGE_FULL_LOAD = 0x0303
}enPackageNum;

/* roller rotation direction */
typedef enum 
{
	ROLLER_STOP = 0x00,
	ROLLER_CLOCKWISE=0x55,
	ROLLER_ANTI_CLOCKWISE=0xCC
}enRollerDirection;


typedef enum 
{
	MOTOR_LEFT =0,
	MOTOR_RIGHT,
	MOTOR_NUT,
	MOTOR_LIFT,		
	MOTOR_RESERVE,	
}enMotorNumber;

typedef enum 
{
	LEFT_MOTOR_INDEX=	0,
	RIGHT_MOTOR_INDEX=	1,
	NUT_MOTOR_INDEX	=	2,
	SCREW_MOTOR_INDEX=	3,
	MOTOR_NUMBER = 2,
}emMotorTypeIndex;

float CalManhattanDistance(const stPoint * p1, const stPoint *p2);

float CalEuclideanDistance(const stPose * p1, const stPose *p2);

/*!< get the square of the x
*@param x, input
*@return x*x
*/
float Square(float x);
 
/*!sort the buffer 
 *@ author:LiuKai
 *@ param dArray, input and output
 *@ param len, length of the dArray buffer
*/
void BubbleSortDouble(double *dArray, int len);
void BubbleSortFloat(float *dArray, int len);
void BubbleSortInt(int *dArray, int len);
void kMinimal(s16* array, int array_nun, int k);
void kMaximum(s16* array, int array_nun, int k);
double LimitAngleDegree(double originalDegAngle);
double AngleMapToOrigin(double curAngle,double preAngle);
double LimitAngleRad(double originalRad);
double LimitAngleDegreePCircle(double originalDegAngle);
double LimitAngleDegreeNCircle(double originalDegAngle);

double ClampDouble(double origin, double up_bound, double low_bound);
float ClampFloat(float origin, float up_bound, float low_bound);
int ClampInt(int origin, int up_bound, int low_bound);
u16 ClampUShort(u16 origin, u16 up_bound, u16 low_bound);
s16 ClampShort(s16 origin, s16 up_bound, s16 low_bound);
double RoundGivenBase(double x, double base);
float RoundGivenBaseF(float x, float base);

/*!angle unit function 
*@ author:LiuKai
*@ param degAngle, input angle at degree unit
*@ return angle at rad unit
*/
double DegToRad(double degAngle);

/*!angle unit function
*@ author:LiuKai
*@ param radAngle, input angle at rad unit
*@ return angle at degree unit
*/
float RadToDeg(double radAngle); 

/*! delay function, short delay,only use in this file, however it's now everywhere!
*@ author:Chenxi
*@ param t, delay time at clock unit
*/
void delay(u32 t);

/*! delay function ,unit ms
*@ author:Chenjiali
*@ param t, delay time at clock unit
*/
void delayms(u32 t);

/*! delay function ,unit us
*@ author:Chenjiali
*@ param t, delay time at clock unit
*/
void delayus(u32 t);

/*! crc check
*@ author:Chenxi
*@ param ptr, input data
*@ param len, input data length
*@return crc result
*/
u16 CalculateCRC16(const u8 *ptr, u8 len);
u16 CheckSumAdd08Anti(const unsigned char *buffer, int length);
bool IsDoubleNumEqual(double x, double y);

enDirection CalRobotDirection(float pose_theta);
void ResetPath(stPathPoints *path); 
bool IsNumbersInSameScope(const float *num, size_t size, float scope);
void LimitIntNumber(int *number, int min_limit_number, int max_limit_number);
bool IsTheSamePoint(const stPoint * p1, const stPoint *p2);
stPoseSensorInfo * GetPoseSensorInfo(void);

#endif
